#include <cmath>

namespace Math
{
    /**
     * 3x3 常用matrix
     */
    class FMatrix3X3
    {
    public:
        float m11, m12, m13;
        float m21, m22, m23;
        float m31, m32, m33;
        FMatrix3X3() = default;
        FMatrix3X3(float m11, float m12, float m13,
                   float m21, float m22, float m23,
                   float m31, float m32, float m33)
            : m11(m11), m12(m12), m13(m13),
              m21(m21), m22(m22), m23(m23),
              m31(m31), m32(m32), m33(m33)
        {
        }
    };

    FMatrix3X3 rotateBank(float b)
    {
        float cb = cos(b);
        float sb = sin(b);
        FMatrix3X3 result{};
        result.m11 = cb, result.m12 = sb, result.m13 = 0.0f;
        result.m21 = -sb, result.m22 = cb, result.m23 = 0.0f;
        result.m31 = 0.0f, result.m32 = 0.0f, result.m33 = 1.0f;
        return result;
    }

    FMatrix3X3 rotatePitch(float p)
    {
        float cp = cos(p);
        float sp = sin(p);
        FMatrix3X3 result(1.0f, 0.0f, 0.0f,
                          0.0f, cp, sp,
                          0.0f, -sp, cp);
        return result;
    }

    FMatrix3X3 rotateHeading(float h)
    {
        float ch = cos(h);
        float sh = sin(h);
        return FMatrix3X3(ch, 0.0f, -sh,
                          0.0f, 1.0f, 0.0f,
                          sh, 0.0f, ch);
    }

    void decomposteRotation(const FMatrix3X3 &rotation, float *head, float *pitch, float *bank)
    {
        // extract picth from m32, being careful for domain errors with
        // asin(). we could have values slightly out of range due to floating point arithmetic
        float sp = -rotation.m32;
        if (sp <= -1.0f)
        {
            *pitch = -1.570796f; // -pi /2
        }
        else if (sp >= 1.0f)
        {
            *pitch = 1.570796f; // pi / 2
        }
        else
        {

            *pitch = asin(sp);
        }

        if (fabs(sp) > 0.9999f) // 防止万向锁情况发生
        {
            //we are looking straight up or down. 
            
            *bank = 0.0f;
            *head = atan2(-rotation.m13, rotation.m11);
        }
        else
        {
            *head = atan2(rotation.m31, rotation.m33);
            *bank = atan2(rotation.m12, rotation.m22);
        }
    }
} // namespace Math
